function [perigee] = RayPerigee(h1, lat1, lon1, h2, lat2, lon2)
%
% Ray-Perigee Determination
%
%DESCRIPTION:
%This function implements the Ray-Perigee problem.
%
%PROTOTYPE:
% [perigee] = RayPerigee(h1, lat1, lon1, h2, lat2, lon2)
%
%--------------------------------------------------------------------------
% INPUTS:
%   h1         [1x1]       Point 1 Altitude          [km]  (User)
%   lat1       [1x1]       Point 1 Latitude          [deg] (User)
%   lon1       [1x1]       Point 1 Longitude         [deg] (User)
%   h2         [1x1]       Point 2 Altitude          [km]  (Sat)
%   lat2       [1x1]       Point 2 Latitude          [deg] (Sat)
%   lon2       [1x1]       Point 2 Longitude         [deg] (Sat)
%--------------------------------------------------------------------------
% OUTPUTS:
%   perigee    [---]       Perigee Results           [strc] (see NOTES)
%--------------------------------------------------------------------------
%
%NOTES:
% - The output "perigee" has been chosen to be a structure (for compactness of
%   the code) defined as:
%       perigee.rp      = Radius           [km]
%       perigee.latp    = Latitude         [deg]
%       perigee.lonp    = Longitude        [deg]
%       perigee.sinlatp = Sine of Latitude [-]
%       perigee.coslatp = Cosine of Latit. [-]
%       perigee.sinsigp = Sine of Zenith   [-]
%       perigee.cossigp = Cosine of Zenith [-]
%
%CALLED FUNCTIONS:
% (none)
%
%UPDATES:
% (none)
%
%REFERENCES:
% [1] "Ionospheric Correction Algorithm for Galileo Single-Frequency Users"
%      - European GNSS (Galileo) Open Service
% [2] "Electron Density Models and Data for Transionospheric Radio
%      Propagation" - Report ITU-R P.2297-1 (05/2019)
%
%AUTHOR(s):
%Luigi De Maria, Matteo D'Addazio, 2022
%

%% Main Code

%Constants
DR = pi/180;                %Conversion Factor: deg->rad
RD = 180/pi;                %Conversion Factor: rad->deg
RE = 6371.2;                %Earth Mean Radius [km]

%Radius
r1 = h1 + RE;
r2 = h2 + RE;

%Zenith Angle Computation [rad] [2.5.8.2.2]
cosdel = sin(lat1*DR)*sin(lat2*DR) + cos(lat1*DR)*cos(lat2*DR)*cos((lon2-lon1)*DR);
if abs(cosdel-1) < 1E-15
     cosdel = 1;
end
sindel = sqrt(1 - cosdel^2);
zeta = atan2(sindel, (cosdel-(r1/r2)));

%Ray Perigee Computation [km] [2.5.8.2.3]
rp = r1 * sin(zeta);

%Perigee Latitude [rad]
if abs(abs(lat1)-90) < 1e-10
    if     lat1 > 0
        latp = zeta;
    elseif lat1 < 0
        latp = -zeta;
    end
else
    %Azimuth of P2 seen from P1
    sinsig = (sin((lon2 - lon1)*DR) * cos(lat2*DR)) / (sindel);
    cossig = (sin(lat2*DR) - cosdel*sin(lat1*DR)) / (sindel*cos(lat1*DR));
    %Earth Angle between P1 and Pp (Ray-Perigee) [rad]
    delp = pi/2 - zeta;
    %Latitude [rad]
    sinlatp = sin(lat1*DR)*cos(delp) - cos(lat1*DR)*sin(delp)*cossig;
    coslatp = sqrt(1 - sinlatp^2);
    latp = atan2(sinlatp, coslatp);
end

%Perigee Longitude [rad]
if abs(abs(lat1)-90) < 1e-10
    if     zeta >= 0
        lonp = lon2*DR + pi;
    elseif zeta < 0
        lonp = lon2*DR;
    end
else
    %Differences
    sindiff = - (sinsig*sin(delp)) / (coslatp);
    cosdiff = (cos(delp) - sin(lat1*DR)*sinlatp) / (cos(lat1*DR)*coslatp);
    %Longitude [rad]
    lonp = (atan2(sindiff, cosdiff) + lon1*DR);
end

%Great Circle Angle [rad] [2.5.8.2.4]
if abs(abs(latp*RD)-90) < 1e-10
    psi = abs(lat2*DR - latp);
else        %coslatp
    cospsi = sinlatp*sin(lat2*DR) + coslatp*cos(lat2*DR)*cos(lon2*DR-lonp);
    sinpsi = sqrt(1 - cospsi^2);
    psi = atan2(sinpsi, cospsi);
end

%Azimuth of Satellite as seen from Ray-Perigee Pp (sigma)
if abs(abs(latp*RD)-90) < 1e-10
    sinsigp = 0;
    if     latp > 0
        cossigp = -1;
    elseif latp < 0
        cossigp =  1;
    end
else         
    sinsigp = + (cos(lat2*DR) * sin(lon2*DR-lonp)) / (sin(psi));
    cossigp = + (sin(lat2*DR) - sinlatp*cos(psi)) / (coslatp*sin(psi));
end

%Conversion for Output [rad]->[deg]
latp = latp * RD;
lonp = lonp * RD;

%Structure for Perigee Data Compactness
perigee.rp      = rp;           %Radius           [km]
perigee.latp    = latp;         %Latitude         [deg]
perigee.lonp    = lonp;         %Longitude        [deg]
perigee.sinlatp = sinlatp;      %Sine of Latitude [-]
perigee.coslatp = coslatp;      %Cosine of Latit. [-]
perigee.sinsigp = sinsigp;      %Sine of Zenith   [-]
perigee.cossigp = cossigp;      %Cosine of Zenith [-]

end